#include "motor.hpp"
#include <vector>
#include <memory>
/**更新各个关节期望角度，以及传感器检测到的实际数据 */

std::vector<std::unique_ptr<MOTOR>> joints;

joints.push_back(std::make_unique<MOTOR_DAMIAO>());
joints.push_back(std::make_unique<MOTOR_LINGKONG>());
joints.push_back(std::make_unique<MOTOR_LINGKONG>());
joints.push_back(std::make_unique<MOTOR_DAMIAO>());
joints.push_back(std::make_unique<MOTOR_DAMIAO>());
joints.push_back(std::make_unique<MOTOR_DJI>());

 void MOTOR_DAMIAO::caculate_and_output() 
{
  MOTOR_DAMIAO::expect_vel=PID_calc(&MOTOR_DAMIAO::pid,MOTOR_DAMIAO::real_pos,MOTOR_DAMIAO::expect_pos);
  if(MOTOR_DAMIAO::can_bus==can1)
  SpeedCtrl(hcan1,MOTOR_DAMIAO::can_id:,MOTOR_DAMIAO::expect_vel);
  else
  SpeedCtrl(hcan2,MOTOR_DAMIAO::can_id:,MOTOR_DAMIAO::expect_vel);
};


 void MOTOR_LINGKONG::caculate_and_output() 
{
 MOTOR_LINGKONG::expect_vel= PID_calc(&MOTOR_LINGKONG::pid,MOTOR_LINGKONG::real_pos,MOTOR_MOTOR_LINGKONGDAMIAO::expect_pos);
  if(MOTOR_LINGKONG::can_bus==can1)
SingleSpeedControl(hcan1,MOTOR_LINGKONG::can_id:,MOTOR_LINGKONG::expect_vel);
  else
SingleSpeedControl(hcan2,MOTOR_LINGKONG::can_id:,MOTOR_LINGKONG::expect_vel);
};


void 

//  void MOTOR_DJI::caculate_and_output() 
// {
//  MOTOR_DJI::expect_vel= PID_calc(&MOTOR_DJI::pid,MOTOR_DJI::real_pos,MOTOR_DJI::expect_pos);
//   if(MOTOR_DJI::can_bus==can1)
// SingleSpeedControl(hcan1,MOTOR_DJI::can_id:,V::expect_vel);
//   else
// SingleSpeedControl(hcan2,MOTOR_DJI::can_id:,V::expect_vel);
// };




























